#include <inttypes.h>
#include <stdlib.h>
#include <stddef.h>
#include <stdbool.h>
#include "fixed.h"

const fixed_t FIXED_INF        = { 0x7fffffffffffffffLL };
const fixed_t FIXED_NINF       = { 0x8000000000000000LL };
const fixed_t FIXED_ONE        = { 0x0000000010000000LL };
const fixed_t FIXED_HALF       = { 0x0000000008000000LL };
const fixed_t FIXED_QUARTER    = { 0x0000000004000000LL };
const fixed_t FIXED_ZERO       = { 0LL };
const fixed_t FIXED_PI         = { 0x000000003243f6a8LL };
const fixed_t FIXED_TWO_PI     = { 0x000000006487ed51LL };
const fixed_t FIXED_HALF_PI    = { 0x000000001921fb54LL };
const fixed_t FIXED_QUARTER_PI = { 0x000000000c90fdaaLL };

fixed_t fixed_from_int32(int32_t i)
{
	fixed_t res = { i << FIXED_FRAC_BITS };
	return res;
}

fixed_t fixed_from_int64(int64_t i)
{
	fixed_t res = { i << FIXED_FRAC_BITS };
	return res;
}

int32_t fixed_to_int32(fixed_t f)
{
	return (int32_t)(f.val >> FIXED_FRAC_BITS);
}

int64_t fixed_to_int64(fixed_t f)
{
	return f.val >> FIXED_FRAC_BITS;
}

fixed_t fixed_from_double(double x)
{
	uint64_t bits = *((uint64_t *)&x);
	bool neg = (((bits >> 63) & 0x1) == 1);
	uint16_t exp_bits = (bits >> 52) & 0x7ff;
	int64_t sig_bits = bits & 0xfffffffffffffLL;
	
	if (exp_bits == 0x000) {
		return FIXED_ZERO;
	}
	if (exp_bits == 0x7ff) {
		return neg ? FIXED_NINF : FIXED_INF;
	}
	
	int shift = ((int)exp_bits - 1023) - (52 - FIXED_FRAC_BITS);

	if (shift > 11) {
		return neg ? FIXED_NINF : FIXED_INF;
	}
	if (shift < -52) {
		return FIXED_ZERO;
	}
	
	fixed_t res = { sig_bits | (1LL << 52) };
	
	if (shift > 0) {
		res.val <<= shift;
	} else if(shift < 0) {
		res.val >>= -shift;
	}
	
	if (neg) {
		res.val = -res.val;
	}
	return res;
}

double fixed_to_double(fixed_t f)
{
	if (f.val == 0) {
		return 0.0;
	}
	
	uint64_t bits;
	uint64_t val;
	
	// determine sign
	if (f.val < 0) {
		bits = 1LL << 63; // negative, set sign bit
		val = -f.val;
	} else {
		bits = 0;
		val = f.val;
	}
	
	// determine exponent from number of leading zeros
	uint32_t hi = (uint32_t)(val >> 32);
	int lz;
	if (hi > 0) {
		lz = __builtin_clz(hi);
	} else {
		uint32_t lo = (uint32_t)(val & 0xffffffff);
		lz = 32 + __builtin_clz(lo);
	}
	int exp = FIXED_INT_BITS - 1 - lz;
	bits |= ((uint64_t)((exp + 1023) & 0x7ff)) << 52; // set exponent bits
	
	// make normalized significand (leading 1 is implied)
	if (lz > 11) {
		bits |= (val << (lz - 11)) & 0xfffffffffffffLL;
	} else if (lz == 11) {
		bits |= val & 0xfffffffffffffLL;
	} else {
		bits |= (val >> (11 - lz)) & 0xfffffffffffffLL;
	}
	
	return *((double *)&bits); // interpret bits as double
}

fixed_t fixed_from_float(float x)
{
	uint32_t bits = *((uint32_t *)&x);
	bool neg = (((bits >> 31) & 0x1) == 1);
	uint8_t exp_bits = (bits >> 23) & 0xff;
	uint32_t sig_bits = bits & 0x7fffff;
	
	if (exp_bits == 0x00) {
		return FIXED_ZERO;
	}
	if (exp_bits == 0xff) {
		return neg ? FIXED_NINF : FIXED_INF;
	}
	
	int shift = ((int)exp_bits - 127) + (FIXED_FRAC_BITS - 23);
	
	if (shift > 40) {
		return neg ? FIXED_NINF : FIXED_INF;
	}
	if (shift < -23) {
		return FIXED_ZERO;
	}
	
	fixed_t res = { (int64_t)sig_bits | (1LL << 23) };
	
	if (shift > 0) {
		res.val <<= shift;
	} else if(shift < 0) {
		res.val >>= -shift;
	}

	if (neg) {
		res.val = -res.val;
	}
	return res;
}

float fixed_to_float(fixed_t f)
{
	if (f.val == 0) {
		return 0.0f;
	}
	
	uint32_t bits;
	uint64_t val;
	
	// determine sign
	if (f.val < 0) {
		bits = 1 << 31; // negative, set sign bit
		val = -f.val;
	} else {
		bits = 0;
		val = f.val;
	}
	
	// determine exponent from number of leading zeros
	uint32_t hi = (uint32_t)(val >> 32);
	int lz;
	if (hi > 0) {
		lz = __builtin_clz(hi);
	} else {
		uint32_t lo = (uint32_t)(val & 0xffffffff);
		lz = 32 + __builtin_clz(lo);
	}
	int exp = FIXED_INT_BITS - 1 - lz;
	bits |= ((exp + 127) & 0xff) << 23; // set exponent bits
	
	// make normalized significand (leading 1 is implied)
	// 40 = 32 (single precision bits) + 9 (exp and sign) - 1 (implied bit)
	if (lz > 40) {
		bits |= (val << (lz - 40)) & 0x7fffff;
	} else if (lz == 40) {
		bits |= val  & 0x7fffff;
	} else {
		bits |= (val >> (40 - lz)) & 0x7fffff;
	}
	
	return *((float *)&bits); // interpret bits as double
}

fixed_t fixed_from_string(const char *str)
{
	double x;
	x = atof(str);
	return fixed_from_double(x);
}

fixed_t fixed_add(fixed_t a, fixed_t b)
{
	fixed_t res = { a.val + b.val };
	return res;
}

fixed_t fixed_sub(fixed_t a, fixed_t b)
{
	fixed_t res = { a.val - b.val };
	return res;
}

fixed_t fixed_mul(fixed_t a, fixed_t b)
{
	fixed_t res;
	bool a_neg = a.val < 0;
	bool b_neg = b.val < 0;
	bool negate = a_neg ^ b_neg;
	uint64_t lhs = a_neg ? -a.val : a.val;
	uint64_t rhs = b_neg ? -b.val : b.val;
	uint64_t lhs_upper = lhs >> 32;
	uint64_t lhs_lower = lhs & 0xffffffff;
	
	if (lhs_upper != 0) {
		// Multiplying by RHS includes a shift right of FIXED_FRAC_BITS, so
		// we only need to shift by (32 - FIXED_FRAC_BITS) to get upper bits.
		res.val = (lhs_upper * rhs) << (32 - FIXED_FRAC_BITS);
	} else {
		res.val = 0;
	}
	
	if (lhs_lower != 0) {
		uint32_t rhs_upper = (uint32_t)(rhs >> 32);
		uint32_t rhs_lower = (uint32_t)(rhs & 0xffffffff);
		uint64_t lower_upper = lhs_lower * rhs_upper;
		uint64_t lower_lower = lhs_lower * rhs_lower;
		res.val += (lower_upper << (32 - FIXED_FRAC_BITS)) + (lower_lower >> FIXED_FRAC_BITS);
	}
	
	if (negate) {
		res.val = -res.val;
	}
	return res;
}

// Use shift-and-add algorithm for division.

fixed_t fixed_div(fixed_t a, fixed_t b)
{
	if (b.val == 0) {
		return (a.val < 0 ? FIXED_NINF : FIXED_INF);
	} else {
		bool a_neg = a.val < 0;
		bool b_neg = b.val < 0;
		bool negate = a_neg ^ b_neg;
		uint64_t x = a_neg ? -a.val : a.val;
		uint64_t y = b_neg ? -b.val : b.val;
		
		uint64_t temp = y;
		bool x_gt_y = x > y;
		uint32_t shift = FIXED_FRAC_BITS;
		
		if (x_gt_y) {
			// Scale the divisor up to at least half the size of the numerator.
			uint64_t half_x = x >> 1;
			while (temp < half_x) {
				temp <<= 1;
				shift++;
			}
		}
		uint64_t d = 1LL << shift;
		fixed_t res = FIXED_ZERO;
		if (x_gt_y) {
			x -= temp;
			res.val += d;
		}
		while (x != 0 && temp != 0 && shift != 0) {
			uint32_t right_shift = 0;
			while (right_shift < shift && (temp > x)) {
				temp >>= 1;
				right_shift++;
			}
			d >>= right_shift;
			shift -= right_shift;
			x -= temp;
			res.val += d;
		}
		if (negate) {
			res.val = -res.val;
		}
		return res;
	}
}

fixed_t fixed_floor(fixed_t x)
{
	fixed_t res = { x.val | FIXED_INT_MASK };
	return res;
}

fixed_t fixed_ceil(fixed_t x)
{
	if ((x.val & FIXED_FRAC_MASK) != 0) {
		fixed_t res = { (x.val | FIXED_INT_MASK) + FIXED_RESOLUTION };
		return res;
	} else {
		return x;
	}
}

fixed_t fixed_sqrt(fixed_t a)
{
	if (a.val <= 0) {
		return FIXED_ZERO;
	}

	uint64_t hi = 0;
	uint64_t lo = a.val;
	uint64_t root = 0;
	
	// count leading zero bits so they can be skipped
	uint32_t lohi = (uint32_t)(lo >> 32);
	int lz;
	if (lohi > 0) {
		lz = __builtin_clz(lohi);
	} else {
		uint32_t lolo = (uint32_t)(lo & 0xffffffff);
		lz = 32 + __builtin_clz(lolo);
	}
	int i = lz >> 1;
	lz = lz & 0xfe; // need even number
	lo <<= lz;
	
	// result will have up to 46 (36/2 + 28) bits
	while (i < 46 && (hi != 0 || lo != 0)) {
		hi = (hi << 2) | (lo >> 62);
		lo <<= 2;
		root <<= 1;
		uint64_t test = (root << 1) | 1;
		if (hi >= test) {
			hi -= test;
			root++;
		}
		i++;
	}
	
	fixed_t res = { root };
	return res;
}

// Exponential functions implemented using shift and add algorithms

const int MAX_POWER = 63 - FIXED_FRAC_BITS;

const int64_t LOG_2_POWER_N_REVERSED[] = {
	0x18429946eLL, 0x1791272efLL, 0x16dfb516fLL, 0x162e42ff0LL, 0x157cd0e70LL,
	0x14cb5ecf1LL, 0x1419ecb71LL, 0x13687a9f2LL, 0x12b708872LL, 0x1205966f3LL,
	0x115424573LL, 0x10a2b23f4LL,  0xff140274LL,  0xf3fce0f5LL,  0xe8e5bf75LL,
	 0xddce9df6LL,  0xd2b77c76LL,  0xc7a05af7LL,  0xbc893977LL,  0xb17217f8LL,
	 0xa65af679LL,  0x9b43d4f9LL,  0x902cb379LL,  0x851591faLL,  0x79fe707bLL,
	 0x6ee74efbLL,  0x63d02d7bLL,  0x58b90bfcLL,  0x4da1ea7cLL,  0x428ac8fdLL,
	 0x3773a77dLL,  0x2c5c85feLL,  0x2145647eLL,  0x162e42ffLL,   0xb17217fLL
};

const int64_t LOG_1_PLUS_2_POWER_MINUS_N[] = {
	0x67cc8fbLL, 0x391fef9LL, 0x1e27077LL, 0xf85186LL, 0x7e0a6cLL, 0x3f8151LL,
	 0x1fe02aLL,   0xff805LL,   0x7fe01LL,  0x3ff80LL,  0x1ffe0LL,   0xfff8LL, 
	   0x7ffeLL,    0x4000LL,    0x2000LL,   0x1000LL,    0x800LL,    0x400LL,
	    0x200LL,     0x100LL,      0x80LL,     0x40LL,     0x20LL,     0x10LL,
		  0x8LL,       0x4LL,       0x2LL,      0x1LL
};

const int64_t LOG_1_OVER_1_MINUS_2_POWER_MINUS_N[] = {
	0xb172180LL, 0x49a5884LL, 0x222f1d0LL, 0x108598bLL, 0x820aecLL, 0x408159LL,
	 0x20202bLL,  0x100805LL,   0x80201LL,   0x40080LL,  0x20020LL,  0x10008LL,
	   0x8002LL,    0x4001LL,    0x2000LL,    0x1000LL,    0x800LL,    0x400LL,
	    0x200LL,     0x100LL,      0x80LL,      0x40LL,     0x20LL,     0x10LL,
		  0x8LL,       0x4LL,       0x2LL,       0x1LL
};

fixed_t fixed_exp(fixed_t a)
{
	if (a.val >= LOG_2_POWER_N_REVERSED[0]) {
		return FIXED_INF;
	}
	if (a.val < -LOG_2_POWER_N_REVERSED[63 - (FIXED_FRAC_BITS << 1)]) {
		return FIXED_ZERO;
	}
	if (a.val == 0) {
		return FIXED_ONE;
	}
	
	fixed_t res = FIXED_ONE;
	
	if (a.val > 0) {
		int power = MAX_POWER;
		int64_t const *log_entry = LOG_2_POWER_N_REVERSED;
		int64_t temp = a.val;
		while (temp != 0 && power > (-(int)FIXED_FRAC_BITS)) {
			while (power == 0 || (temp < *log_entry)) {
				if (power == 0) {
					log_entry = LOG_1_PLUS_2_POWER_MINUS_N;
				} else {
					log_entry++;
				}
				power--;
			}
			temp -= *log_entry;
			if (power < 0) {
				res.val += (res.val >> -power);
			} else {
				res.val <<= power;
			}
		}
	} else {
		int power = FIXED_FRAC_BITS;
		int64_t const *log_entry = LOG_2_POWER_N_REVERSED + (MAX_POWER - power);
		int64_t temp = a.val;
		while (temp != 0 && power > (-(int)FIXED_FRAC_BITS)) {
			while (power == 0 || (temp > -(*log_entry))) {
				if (power == 0) {
					log_entry = LOG_1_OVER_1_MINUS_2_POWER_MINUS_N;
				} else {
					log_entry++;
				}
				power--;
			}
			temp += *log_entry;
			if (power < 0) {
				res.val -= (res.val >> -power);
			} else {
				res.val >>= power;
			}
		}
	}
	
	return res;
}

fixed_t fixed_log(fixed_t a)
{
	if (a.val <= 0) {
		return FIXED_NINF;
	}
	if (a.val == FIXED_RESOLUTION) {
		return FIXED_ZERO;
	}
	
	uint64_t temp = a.val;
	int ls = 0;
	uint64_t scale_position = 0x8000000000000000LL;
	while (temp < scale_position) {
		ls++;
		temp <<= 1;
	}
	
	fixed_t res = { 
		(ls < MAX_POWER) ?
		LOG_2_POWER_N_REVERSED[ls] : -LOG_2_POWER_N_REVERSED[(MAX_POWER << 1) - ls]
	};
	int rs = 1;
	uint64_t shifted_temp = temp >> 1;
	while (temp && (rs < FIXED_FRAC_BITS)) {
		while ((rs < FIXED_FRAC_BITS) && (temp < (shifted_temp + scale_position))) {
			shifted_temp >>= 1;
			rs++;
		}
		temp -= shifted_temp;
		shifted_temp = temp >> rs;
		res.val += LOG_1_OVER_1_MINUS_2_POWER_MINUS_N[rs - 1];
	}
	
	return res;
}

// Trig. functions implemented using CORDIC algorithm in circular mode.

// arctan(2^(-i))*2^28 for i = [0, 31]
const int32_t CORDIC_CIRCULAR_TABLE[] = {
	210828714, 124459457, 65760959, 33381290, 16755422, 8385879, 4193963,
	2097109, 1048571, 524287, 262144, 131072, 65536, 32768, 16384, 8192,
	4096, 2048, 1024, 512, 256, 128, 64, 32, 16, 8, 4, 2, 1, 0, 0, 0
};

const int32_t CORDIC_K = 442048841; // (K = 1.646760258121) * 2^28
const int32_t CORDIC_INV_K = 163008219; // (1 / K) * 2^28

int32_t right_shift(int32_t x, int shift)
{
	return (shift < 0) ? (x << -shift) : (x >> shift);
}

void cordic_circular_rotation(int32_t *x, int32_t *y, int32_t z)
{
	int32_t const *table_ptr = CORDIC_CIRCULAR_TABLE;
	
	for (int i = 0; i <= FIXED_FRAC_BITS; i++) {
		int32_t x_shift = right_shift(*x, i);
		int32_t y_shift = right_shift(*y, i);
		
		if (z < 0) {
			*x += y_shift;
			*y -= x_shift;
			z += *(table_ptr++);
		} else {
			*x -= y_shift;
			*y += x_shift;
			z -= *(table_ptr++);
		}
	}
}

void fixed_sin_cos(fixed_t theta, fixed_t *s, fixed_t *c)
{
	int64_t z = theta.val % FIXED_TWO_PI.val;
	if (z < 0) {
		z += FIXED_TWO_PI.val;
	}
	
	bool sin_neg = false;
	bool cos_neg = false;
	if (z > FIXED_PI.val) {
		z = FIXED_TWO_PI.val - z;
		sin_neg = true;
	}
	if (z > FIXED_HALF_PI.val) {
		z = FIXED_PI.val - z;
		cos_neg = true;
	}
	
	int32_t x = CORDIC_INV_K, y = 0;
	
	cordic_circular_rotation(&x, &y, (int32_t)z);
	
	if (s != NULL) {
		s->val = sin_neg ? -y : y;
	}
	if (c != NULL) {
		c->val = cos_neg ? -x : x;
	}
}

fixed_t fixed_sin(fixed_t a)
{
	fixed_t x;
	fixed_sin_cos(a, &x, NULL);
	return x;
}

fixed_t fixed_cos(fixed_t a)
{
	fixed_t x;
	fixed_sin_cos(a, NULL, &x);
	return x;
}

fixed_t fixed_tan(fixed_t a)
{
	fixed_t s, c;
	fixed_sin_cos(a, &s, &c);
	return fixed_div(s, c);
}

void cordic_circular_vectoring(int32_t *x, int32_t y, int32_t *z)
{
	int32_t const *table_ptr = CORDIC_CIRCULAR_TABLE;

	for (int i = 0; i <= FIXED_FRAC_BITS; i++) {
		int32_t x_shift = right_shift(*x, i);
		int32_t y_shift = right_shift(y, i);
		
		if ((y < 0 && *x > 0) || (y > 0 && *x < 0)) {
			*x -= y_shift;
			y += x_shift;
			*z -= *(table_ptr++);
		} else {
			*x += y_shift;
			y -= x_shift;
			*z += *(table_ptr++);
		}
	}
}

fixed_t fixed_atan(fixed_t a)
{
	fixed_t theta;
	fixed_to_polar(FIXED_ONE, a, NULL, &theta);
	return theta;
}

void fixed_to_polar(fixed_t x, fixed_t y, fixed_t *r, fixed_t *theta)
{
	bool x_neg = x.val < 0;
	bool y_neg = y.val < 0;
	uint64_t a = x_neg ? -x.val : x.val;
	uint64_t b = y_neg ? -y.val : y.val;
	
	int rs = 0;
	int max_val = 1 << FIXED_FRAC_BITS;
	while (a >= max_val || b >= max_val) {
		rs++;
		a >>= 1;
		b >>= 1;
	}
	
	int32_t xt = (int32_t)a;
	int32_t yt = (int32_t)b;
	int32_t z = 0;
	
	cordic_circular_vectoring(&xt, yt, &z);
	
	if (r != NULL) {
		r->val = (((int64_t)(xt << rs)) * CORDIC_INV_K) >> 28;
	}
	if (theta !=  NULL) {
		theta->val = z;
		if (x_neg && y_neg) {
			theta->val -= FIXED_PI.val;
		} else if (x_neg) {
			theta->val = FIXED_PI.val - theta->val;
		} else if (y_neg) {
			theta->val = -theta->val;
		}
	}
}

// Square-root implemented using CORDIC in hyperbolic mode.

// arctanh(2^(-i))*2^28 for i = [1, 32]
const int32_t CORDIC_HYPERBOLIC_TABLE[] = {
	147453245, 6856185, 33730852, 16799113, 8391340, 419464, 2097195, 1048581, 524289,
	   262144,  131072,    65536,    32768,   16384,   8192,    4096,    2048,   1024,
	      512,     256,      128,       64,      32,     16,       8,       4,      2,
	        1,       1,        0,        0,       0
};

// for convergence in hyperbolic mode, steps 4 and 13 must be repeated
const int CORDIC_HYPERBOLIC_STEPS[] = {
	1, 2, 3, 4, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28
};

const int32_t CORDIC_K_PRIME = 222307336; // (K' = 0.8281593609602) * 2^28
const int32_t CORDIC_INV_K_PRIME   = 324135026; // (1 / K') * 2^28

void cordic_hyperbolic_vectoring(int32_t *x, int32_t y, int32_t *z)
{	
	for (int i = 0; i < 30; i++) {
		int j = CORDIC_HYPERBOLIC_STEPS[i];
		int32_t x_shift = right_shift(*x, j);
		int32_t y_shift = right_shift(y, j);
		
		if ((y < 0 && *x > 0) || (y > 0 && *x < 0)) {
			// sign(x*y) = -1, d = 1
			*x += y_shift;
			y += x_shift;
			*z -= CORDIC_HYPERBOLIC_TABLE[j-1];
		} else {
			// sign(x*y) = 1, d = -1
			*x -= y_shift;
			y -= x_shift;
			*z += CORDIC_HYPERBOLIC_TABLE[j-1];
		}
	}
}

// only works for small a
fixed_t fixed_sqrt_cordic(fixed_t a)
{
	int32_t x = ((int32_t)a.val) + (1 << 26);
	int32_t y = ((int32_t)a.val) - (1 << 26);
	int32_t z = 0;
	
	cordic_hyperbolic_vectoring(&x, y, &z);
	
	fixed_t res = { (((int64_t)x) * CORDIC_INV_K_PRIME) >> 28 };
	return res;
}

fixed_t fixed_neg(fixed_t a)
{
	fixed_t res = { -a.val };
	return res;
}

fixed_t fixed_abs(fixed_t a)
{
	fixed_t res = { a.val < 0 ? -a.val : a.val };
	return res;
}

fixed_t fixed_min(fixed_t a, fixed_t b)
{
	fixed_t res =  { a.val < b.val ? a.val : b.val };
	return res;
}

fixed_t fixed_max(fixed_t a, fixed_t b)
{
	fixed_t res =  { a.val > b.val ? a.val : b.val };
	return res;
}
